#include <errno.h>
#include "my_include.h"
#include "my_fifo.h"
#include "log.h"

//pthread_mutex_t spinLock = PTHREAD_MUTEX_INITIALIZER;


inline U32 SFifoCanReadNum( PSFIFO pSFifo )
{
	return pSFifo->ulCanReadNumber;
}

inline U32 SFifoCanWriteNum( PSFIFO pSFifo )
{
	return pSFifo->ulSize - pSFifo->ulCanReadNumber;
}

int SFifoWriteSingle( PSFIFO pSFifo, U16 usData )
{
	if( !pSFifo ) return -1;
	if( pSFifo->ulCanReadNumber == pSFifo->ulSize ) return 0;
	pSFifo->pusData[pSFifo->ulWriteInx] = usData;
	pSFifo->ulCanReadNumber++;
	pSFifo->ulWriteInx++;
	if( pSFifo->ulWriteInx == pSFifo->ulSize )
		pSFifo->ulWriteInx = 0;
	return sizeof(usData);
}

int SFifoWrite( PSFIFO pSFifo, U16* pusData, U32 ulWriteNumber )
{
	unsigned int l;

	ulWriteNumber = min( ulWriteNumber, pSFifo->ulSize - pSFifo->ulCanReadNumber );

	l = min( ulWriteNumber, pSFifo->ulSize - pSFifo->ulWriteInx );
	memcpy( pSFifo->pusData + pSFifo->ulWriteInx, pusData, l * sizeof(U16) );
	memcpy( pSFifo->pusData, pusData + l, (ulWriteNumber - l) * sizeof(U16) );

	l = pSFifo->ulWriteInx + ulWriteNumber;
	pSFifo->ulWriteInx = l < pSFifo->ulSize ? 
						l :
						l - pSFifo->ulSize;
	pSFifo->ulCanReadNumber += ulWriteNumber;
//	//log_write( "SFifoWrite:%d", pSFifo->ulCanReadNumber );

	return ulWriteNumber;
}

int SFifoWriteLock( PSFIFO pSFifo, U16* pusData, U32 ulWriteNumber )
{
	int ret;
//	//log_write( "SFifoWriteLock" );
	pthread_mutex_lock( &pSFifo->spinLock );
	ret = SFifoWrite( pSFifo, pusData, ulWriteNumber );
	if (pSFifo->ulWantReadNumber>0 && pSFifo->ulCanReadNumber >= pSFifo->ulWantReadNumber)
	{
		pSFifo->ulWantReadNumber = 0;
		pthread_cond_signal(&pSFifo->fifo_cond);
	}
	pthread_mutex_unlock( &pSFifo->spinLock );
//	//log_write( "<1>W:%p", &pSFifo->spinLock );
	return ret;
}

int SFifoRead( PSFIFO pSFifo, U16* pusData, U32 ulReadNumber )
{
	unsigned int l;

	ulReadNumber = min( ulReadNumber, pSFifo->ulCanReadNumber );

	if( pusData )
	{
		l = min( ulReadNumber, pSFifo->ulSize - pSFifo->ulReadInx );
		memcpy( pusData, pSFifo->pusData + pSFifo->ulReadInx, l * sizeof(U16) );
		memcpy( pusData + l, pSFifo->pusData, (ulReadNumber - l) * sizeof(U16) );
	}

	l = pSFifo->ulReadInx + ulReadNumber;
	pSFifo->ulReadInx = l < pSFifo->ulSize ? 
						l : 
						l - pSFifo->ulSize;
	pSFifo->ulCanReadNumber -= ulReadNumber;

	return ulReadNumber;
}

int SFifoReadLock( PSFIFO pSFifo, U16* pusData, U32 ulReadNumber )
{
	int ret;
	pthread_mutex_lock( &pSFifo->spinLock );
	if (pSFifo->ulCanReadNumber >= ulReadNumber)
		ret = SFifoRead( pSFifo, pusData, ulReadNumber );
	else // data is less, block thread wait for data.
	{
		struct timespec outtime;
		pSFifo->ulWantReadNumber = ulReadNumber;
	    outtime.tv_sec = 1;
	    outtime.tv_nsec = 0;
		ret = pthread_cond_timedwait(&pSFifo->fifo_cond, &pSFifo->spinLock, &outtime);
		if (ret != ETIMEDOUT)
			ret = SFifoRead( pSFifo, pusData, ulReadNumber );
		else
			ret = -ETIMEDOUT;
	}
	pthread_mutex_unlock( &pSFifo->spinLock );
//	//log_write( "SFifoReadLock" );
	return ret;
}

void SFifoClear( PSFIFO pSFifo )
{
	pthread_mutex_lock( &pSFifo->spinLock );
	pSFifo->ulCanReadNumber = 0;
	pSFifo->ulReadInx = 0;
	pSFifo->ulWriteInx = 0;
	pSFifo->ulWantReadNumber = 0;
	pthread_mutex_unlock( &pSFifo->spinLock );
}

PSFIFO SFifoAlloc( U32 ulSize )
{
	PSFIFO pSFifo = (PSFIFO)malloc( sizeof(SFIFO) );
	if( !pSFifo ) return NULL;
	pSFifo->pusData = malloc( sizeof(U16) * ulSize );
	if( !pSFifo->pusData ){
		//log_write("SFifoAlloc:pusData == NULL");
		free( pSFifo );
		return NULL;
	}
	memset( pSFifo->pusData, 0, sizeof(U16) * ulSize );
	pSFifo->ulCanReadNumber = 0;
	pSFifo->ulReadInx = 0;
	pSFifo->ulWriteInx = 0;
	pSFifo->overFlow = 0;
	pSFifo->ulWantReadNumber = 0;
	pSFifo->ulSize = ulSize;
	pthread_mutex_init( &pSFifo->spinLock, PTHREAD_MUTEX_TIMED_NP );
	pthread_cond_init(&pSFifo->fifo_cond, NULL);
//	//log_write( "<1>S:%p", &pSFifo->spinLock );
	return pSFifo;
}

void SFifoFree( PSFIFO psf )
{
	if( !psf ) return;
	pthread_mutex_destroy( &psf->spinLock );
	if( psf->pusData )
		free( psf->pusData );
	psf->pusData = NULL;
	free( psf );
}
